import java.awt.geom.Point2D;
import java.util.List;

/**
 * The systems reactive module. Basically a collision detection module which the
 * Navigator uses while moving the robot.
 * 
 * @author Emanuel Yifter Lindgren (dv11elr), Anders Lumio (dv11alo)
 *
 */
public class AvoidanceReactor implements Reactor {
    private static double CIRCLE_RADIUS = 0.4;
    private static double MAX_DETECTION_RANGE = 2;
    private final double[] laserAngles;

    public AvoidanceReactor(final double[] laserAngles) {
        this.laserAngles = laserAngles;
    }

    /*
     * (non-Javadoc)
     * 
     * @see Reactor#isCollisionDetected(java.awt.geom.Point2D.Double, double,
     * double[], java.util.List)
     */
    @Override
    public boolean isCollisionDetected(final Point2D.Double robotPos,
            final double robotAngle, final double[] echoes,
            final List<Point2D.Double> navPath) {

        // We check each echo if it bounced on a position that is too close to a
        // point on our path. If it is, then a collision is inevitable if the
        // robot drives along the path.
        for (int i = 0; i < echoes.length; i++) {
            final double echoDistance = echoes[i];

            if (echoDistance > MAX_DETECTION_RANGE) {
                continue;
            }

            final double echoAngle = laserAngles[i];

            final double errorAngle = robotAngle + echoAngle;

            final double bounceX = robotPos.y
                    + (Math.sin(errorAngle) * echoDistance);
            final double bounceY = robotPos.x
                    + (Math.cos(errorAngle) * echoDistance);

            for (final Point2D.Double pPath : navPath) {
                if (pPath.distance(bounceX, bounceY) < CIRCLE_RADIUS) {

                    System.out.println("Collision predicted at-> x: " + bounceX
                            + " y: " + bounceY);
                    return true;
                }
            }

        }

        return false;
    }
}
